<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.6"/>
<title>easykf-2.04: example-009.cc</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td style="padding-left: 0.5em;">
   <div id="projectname">easykf-2.04
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.6 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
</div><!-- top -->
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark">&#160;</span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark">&#160;</span>Enumerator</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(8)"><span class="SelectionMark">&#160;</span>Macros</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(9)"><span class="SelectionMark">&#160;</span>Pages</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">example-009.cc</div>  </div>
</div><!--header-->
<div class="contents">
<div class="fragment"><div class="line"><span class="comment">/* example-009.cc</span></div>
<div class="line"><span class="comment"> * </span></div>
<div class="line"><span class="comment"> * Copyright (C) 2011-2014 Jeremy Fix</span></div>
<div class="line"><span class="comment"> * </span></div>
<div class="line"><span class="comment"> * This program is free software; you can redistribute it and/or modify</span></div>
<div class="line"><span class="comment"> * it under the terms of the GNU General Public License as published by</span></div>
<div class="line"><span class="comment"> * the Free Software Foundation; either version 3 of the License, or (at</span></div>
<div class="line"><span class="comment"> * your option) any later version.</span></div>
<div class="line"><span class="comment"> * </span></div>
<div class="line"><span class="comment"> * This program is distributed in the hope that it will be useful, but</span></div>
<div class="line"><span class="comment"> * WITHOUT ANY WARRANTY; without even the implied warranty of</span></div>
<div class="line"><span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU</span></div>
<div class="line"><span class="comment"> * General Public License for more details.</span></div>
<div class="line"><span class="comment"> * </span></div>
<div class="line"><span class="comment"> * You should have received a copy of the GNU General Public License</span></div>
<div class="line"><span class="comment"> * along with this program; if not, write to the Free Software</span></div>
<div class="line"><span class="comment"> * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.</span></div>
<div class="line"><span class="comment"> */</span></div>
<div class="line"></div>
<div class="line"><span class="comment">/* In this example, we use EKF for state/parameter estimation in order to estimate the state and parameters of a Lorentz attractor */</span></div>
<div class="line"></div>
<div class="line"><span class="preprocessor">#include &lt;cstdlib&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;cmath&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;iostream&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;fstream&gt;</span></div>
<div class="line"><span class="preprocessor">#include &quot;<a class="code" href="ukf_8h.html">ukf.h</a>&quot;</span></div>
<div class="line"></div>
<div class="line"><span class="preprocessor">#include &lt;sys/time.h&gt;</span></div>
<div class="line"></div>
<div class="line"><span class="keyword">using namespace </span>ekf;</div>
<div class="line"></div>
<div class="line"><span class="preprocessor">#define SIGMA 10.0</span></div>
<div class="line"><span class="preprocessor"></span><span class="preprocessor">#define RHO 28.0</span></div>
<div class="line"><span class="preprocessor"></span><span class="preprocessor">#define BETA 8.0/3.0</span></div>
<div class="line"><span class="preprocessor"></span><span class="preprocessor">#define DT 0.01</span></div>
<div class="line"><span class="preprocessor"></span></div>
<div class="line"><span class="preprocessor">#define X0 0.1</span></div>
<div class="line"><span class="preprocessor"></span><span class="preprocessor">#define Y0 1.2</span></div>
<div class="line"><span class="preprocessor"></span><span class="preprocessor">#define Z0 0.4</span></div>
<div class="line"><span class="preprocessor"></span></div>
<div class="line"><span class="preprocessor">#define NOISE_AMPLITUDE 2.0</span></div>
<div class="line"><span class="preprocessor"></span></div>
<div class="line"><span class="preprocessor">#define VERBOSE true</span></div>
<div class="line"><span class="preprocessor"></span></div>
<div class="line"><span class="comment">/*************************************************************************************/</span></div>
<div class="line"><span class="comment">/*            Definition of the evolution and observation functions                  */</span></div>
<div class="line"><span class="comment">/*************************************************************************************/</span></div>
<div class="line"></div>
<div class="line"><span class="comment">// Evolution function</span></div>
<div class="line"><span class="keywordtype">void</span> f(gsl_vector * params, gsl_vector * xk_1, gsl_vector * xk)</div>
<div class="line">{</div>
<div class="line">    <span class="keywordtype">double</span> x = gsl_vector_get(xk_1,0);</div>
<div class="line">    <span class="keywordtype">double</span> y = gsl_vector_get(xk_1,1);</div>
<div class="line">    <span class="keywordtype">double</span> z = gsl_vector_get(xk_1,2);</div>
<div class="line"></div>
<div class="line">    <span class="keywordtype">double</span> sigma = gsl_vector_get(xk_1,3);</div>
<div class="line">    <span class="keywordtype">double</span> rho = gsl_vector_get(xk_1,4);</div>
<div class="line">    <span class="keywordtype">double</span> beta = gsl_vector_get(xk_1,5);</div>
<div class="line"></div>
<div class="line">    <span class="keywordtype">double</span> dt = gsl_vector_get(params, 0);</div>
<div class="line"></div>
<div class="line">    gsl_vector_set(xk, 0, x + dt * (sigma*( y - x)));</div>
<div class="line">    gsl_vector_set(xk, 1, y + dt * ( rho * x - y - x * z));</div>
<div class="line">    gsl_vector_set(xk, 2, z + dt * (x * y - beta * z));</div>
<div class="line">    gsl_vector_set(xk, 3, sigma);</div>
<div class="line">    gsl_vector_set(xk, 4, rho);</div>
<div class="line">    gsl_vector_set(xk, 5, beta);</div>
<div class="line">}</div>
<div class="line"></div>
<div class="line"><span class="comment">// Jacobian of the evolution function</span></div>
<div class="line"><span class="keywordtype">void</span> df(gsl_vector * params, gsl_vector * xk_1, gsl_matrix * Fxk)</div>
<div class="line">{</div>
<div class="line">    <span class="keywordtype">double</span> x = gsl_vector_get(xk_1,0);</div>
<div class="line">    <span class="keywordtype">double</span> y = gsl_vector_get(xk_1,1);</div>
<div class="line">    <span class="keywordtype">double</span> z = gsl_vector_get(xk_1,2);</div>
<div class="line"></div>
<div class="line">    <span class="keywordtype">double</span> sigma = gsl_vector_get(xk_1,3);</div>
<div class="line">    <span class="keywordtype">double</span> rho = gsl_vector_get(xk_1,4);</div>
<div class="line">    <span class="keywordtype">double</span> beta = gsl_vector_get(xk_1,5);</div>
<div class="line"></div>
<div class="line">    <span class="keywordtype">double</span> dt = gsl_vector_get(params, 0);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for x(t+1) = ..</span></div>
<div class="line">    gsl_matrix_set(Fxk, 0, 0, 1.0 - sigma * dt);</div>
<div class="line">    gsl_matrix_set(Fxk, 0, 1, sigma * dt);</div>
<div class="line">    gsl_matrix_set(Fxk, 0, 2, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 0, 3, dt * (y - x));</div>
<div class="line">    gsl_matrix_set(Fxk, 0, 4, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 0, 5, 0.0);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for y(t+1)</span></div>
<div class="line">    gsl_matrix_set(Fxk, 1, 0, dt * (rho - z));</div>
<div class="line">    gsl_matrix_set(Fxk, 1, 1, 1.0 - dt);</div>
<div class="line">    gsl_matrix_set(Fxk, 1, 2, - dt * x);</div>
<div class="line">    gsl_matrix_set(Fxk, 1, 3, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 1, 4, dt * x);</div>
<div class="line">    gsl_matrix_set(Fxk, 1, 5, 0.0);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for z(t+1)</span></div>
<div class="line">    gsl_matrix_set(Fxk, 2, 0, dt * y);</div>
<div class="line">    gsl_matrix_set(Fxk, 2, 1, dt * x);</div>
<div class="line">    gsl_matrix_set(Fxk, 2, 2, 1.0 - beta * dt);</div>
<div class="line">    gsl_matrix_set(Fxk, 2, 3, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 2, 4, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 2, 5, -dt * z);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for sigma(t+1)</span></div>
<div class="line">    gsl_matrix_set(Fxk, 3, 0, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 3, 1, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 3, 2, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 3, 3, 1.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 3, 4, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 3, 5, 0.0);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for rho(t+1)</span></div>
<div class="line">    gsl_matrix_set(Fxk, 4, 0, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 4, 1, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 4, 2, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 4, 3, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 4, 4, 1.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 4, 5, 0.0);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Derivatives for beta(t+1)</span></div>
<div class="line">    gsl_matrix_set(Fxk, 5, 0, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 5, 1, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 5, 2, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 5, 3, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 5, 4, 0.0);</div>
<div class="line">    gsl_matrix_set(Fxk, 5, 5, 1.0);</div>
<div class="line">}</div>
<div class="line"></div>
<div class="line"></div>
<div class="line"><span class="comment">// Observation function</span></div>
<div class="line"><span class="keywordtype">void</span> h(gsl_vector * params, gsl_vector * xk , gsl_vector * yk)</div>
<div class="line">{</div>
<div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i &lt; yk-&gt;size ; ++i)</div>
<div class="line">        gsl_vector_set(yk, i, gsl_vector_get(xk,i) + NOISE_AMPLITUDE*rand()/ <span class="keywordtype">double</span>(RAND_MAX));</div>
<div class="line">}</div>
<div class="line"></div>
<div class="line"><span class="comment">// Jacobian of the observation function</span></div>
<div class="line"><span class="keywordtype">void</span> dh(gsl_vector * params, gsl_vector * xk , gsl_matrix * Hyk)</div>
<div class="line">{</div>
<div class="line">    gsl_matrix_set_zero(Hyk);</div>
<div class="line">    gsl_matrix_set(Hyk, 0, 0, 1.0);</div>
<div class="line">    gsl_matrix_set(Hyk, 1, 1, 1.0);</div>
<div class="line">    gsl_matrix_set(Hyk, 2, 2, 1.0);</div>
<div class="line">}</div>
<div class="line"></div>
<div class="line"><span class="comment">/*****************************************************/</span></div>
<div class="line"><span class="comment">/*                    main                           */</span></div>
<div class="line"><span class="comment">/*****************************************************/</span></div>
<div class="line"></div>
<div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>* argv[]) {</div>
<div class="line"></div>
<div class="line">    <span class="keyword">struct </span>timeval before, after;</div>
<div class="line"></div>
<div class="line">    srand(time(NULL));</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Definition of the parameters and state variables</span></div>
<div class="line">    <a name="_a0"></a><a class="code" href="structekf_1_1ekf__param.html">ekf_param</a> p;</div>
<div class="line">    <a name="_a1"></a><a class="code" href="structekf_1_1ekf__state.html">ekf_state</a> s;</div>
<div class="line">    <span class="comment">// The parameters for the evolution equation</span></div>
<div class="line">    s.<a name="a2"></a><a class="code" href="structekf_1_1ekf__state.html#abe2839dd3805e4dd56c613227788606f">params</a> = gsl_vector_alloc(1);</div>
<div class="line">    s.<a class="code" href="structekf_1_1ekf__state.html#abe2839dd3805e4dd56c613227788606f">params</a>-&gt;data[0] = DT;</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Initialization of the parameters</span></div>
<div class="line">    p.<a name="a3"></a><a class="code" href="structekf_1_1ekf__param.html#a1246cf99fd3c8a31199c74c2ebc96574">n</a> = 6;</div>
<div class="line">    p.<a name="a4"></a><a class="code" href="structekf_1_1ekf__param.html#a1430eac7209095599a396000fc31ec8c">no</a> = 3;</div>
<div class="line"></div>
<div class="line">    <span class="comment">//EvolutionNoise * evolution_noise = new ekf::EvolutionAnneal(1e-2, 0.99, 1e-8);</span></div>
<div class="line">    <a name="_a5"></a><a class="code" href="classekf_1_1EvolutionNoise.html">EvolutionNoise</a> * evolution_noise = <span class="keyword">new</span> <a name="_a6"></a><a class="code" href="classekf_1_1EvolutionRLS.html">ekf::EvolutionRLS</a>(1e-2, 0.9995);</div>
<div class="line">    <span class="comment">//EvolutionNoise * evolution_noise = new ekf::EvolutionRobbinsMonro(1e-5, 1e-6);</span></div>
<div class="line">    p.<a name="a7"></a><a class="code" href="structekf_1_1ekf__param.html#a9b71dc086b83e78c78f6258ce19fafb4">evolution_noise</a> = evolution_noise;</div>
<div class="line"></div>
<div class="line">    p.<a name="a8"></a><a class="code" href="structekf_1_1ekf__param.html#a931255569a953be75868b7ef098ff310">observation_noise</a> = 1e-1;</div>
<div class="line"></div>
<div class="line">    p.<a name="a9"></a><a class="code" href="structekf_1_1ekf__param.html#afc3c7b95fe943c99c49e7b0c5703de68">prior_pk</a> = 1.0;</div>
<div class="line">    p.<a name="a10"></a><a class="code" href="structekf_1_1ekf__param.html#a8e3a67547cde4646e17689a669fb25bc">observation_gradient_is_diagonal</a> = <span class="keyword">true</span>;</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Initialization of the state and parameters</span></div>
<div class="line">    <a name="a11"></a><a class="code" href="namespaceekf.html#a11045811ee6b53e5676adcb4829445de">ekf_init</a>(p,s);</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Initialize the parameter vector to some random values</span></div>
<div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; p.<a class="code" href="structekf_1_1ekf__param.html#a1246cf99fd3c8a31199c74c2ebc96574">n</a> ; i++)</div>
<div class="line">        gsl_vector_set(s.<a name="a12"></a><a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>,i,5.0*(2.0*rand()/<span class="keywordtype">double</span>(RAND_MAX-1)-1.0));</div>
<div class="line">    s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[0] = -15.0;</div>
<div class="line">    s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[1] = -15.0;</div>
<div class="line">    s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[2] = -15.0;</div>
<div class="line"></div>
<div class="line">    <span class="comment">// Allocate the input/output vectors</span></div>
<div class="line">    gsl_vector * xi = gsl_vector_alloc(p.<a class="code" href="structekf_1_1ekf__param.html#a1246cf99fd3c8a31199c74c2ebc96574">n</a>);</div>
<div class="line">    gsl_vector * yi = gsl_vector_alloc(p.<a class="code" href="structekf_1_1ekf__param.html#a1430eac7209095599a396000fc31ec8c">no</a>);</div>
<div class="line">    gsl_vector_set_zero(yi);</div>
<div class="line"></div>
<div class="line">    <span class="comment">/***********************************************/</span></div>
<div class="line">    <span class="comment">/***** Iterate the learning on the samples *****/</span></div>
<div class="line">    <span class="comment">/***********************************************/</span></div>
<div class="line"></div>
<div class="line">    <span class="keywordtype">int</span> epoch = 0;</div>
<div class="line"></div>
<div class="line">    xi-&gt;data[0] = X0;</div>
<div class="line">    xi-&gt;data[1] = Y0;</div>
<div class="line">    xi-&gt;data[2] = Z0;</div>
<div class="line">    xi-&gt;data[3] = SIGMA;</div>
<div class="line">    xi-&gt;data[4] = RHO;</div>
<div class="line">    xi-&gt;data[5] = BETA;</div>
<div class="line"></div>
<div class="line">    std::ofstream outfile(<span class="stringliteral">&quot;example-009.data&quot;</span>);</div>
<div class="line">    std::ofstream outfile_rms(<span class="stringliteral">&quot;example-009-rms.data&quot;</span>);</div>
<div class="line">    <span class="keywordtype">double</span> rms= 0.0;</div>
<div class="line">    <span class="keywordtype">double</span> errorBound = NOISE_AMPLITUDE/2.0;</div>
<div class="line">    <span class="keywordtype">int</span> count_time=0;</div>
<div class="line">    <span class="keywordtype">int</span> nb_steps_below_error=1000;</div>
<div class="line">    <span class="comment">//int is_counting=0;</span></div>
<div class="line"></div>
<div class="line">    gettimeofday(&amp;before, NULL);</div>
<div class="line"></div>
<div class="line">    outfile &lt;&lt; epoch &lt;&lt; <span class="stringliteral">&quot; &quot;</span>;</div>
<div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 6 ; ++i)</div>
<div class="line">        outfile &lt;&lt; xi-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 3 ; ++i)</div>
<div class="line">        outfile &lt;&lt; yi-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 6 ; ++i)</div>
<div class="line">        outfile &lt;&lt; s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">    outfile &lt;&lt; std::endl;</div>
<div class="line"></div>
<div class="line">    <span class="keywordflow">while</span>( epoch &lt; 8000)</div>
<div class="line">    {</div>
<div class="line">        <span class="comment">//printf(&quot;Epoch %i \n&quot;, epoch);</span></div>
<div class="line">        <span class="comment">// Evaluate the true dynamical system</span></div>
<div class="line">        f(s.<a class="code" href="structekf_1_1ekf__state.html#abe2839dd3805e4dd56c613227788606f">params</a>, xi, xi);</div>
<div class="line">        h(s.<a class="code" href="structekf_1_1ekf__state.html#abe2839dd3805e4dd56c613227788606f">params</a>, xi, yi);</div>
<div class="line">        <span class="comment">// Provide the observation and iterate</span></div>
<div class="line">        <a name="a13"></a><a class="code" href="namespaceekf.html#a59a2f5e87de993d4320c6c2492ab03f1">ekf_iterate</a>(p,s,f,df,h,dh,yi);</div>
<div class="line"></div>
<div class="line">        epoch++;</div>
<div class="line"></div>
<div class="line">        rms = 0.0;</div>
<div class="line">        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; p.<a class="code" href="structekf_1_1ekf__param.html#a1246cf99fd3c8a31199c74c2ebc96574">n</a> ; ++i)</div>
<div class="line">            rms += gsl_pow_2(xi-&gt;data[i] - s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[i]);</div>
<div class="line">        rms /= double(p.<a class="code" href="structekf_1_1ekf__param.html#a1246cf99fd3c8a31199c74c2ebc96574">n</a>);</div>
<div class="line">        rms = sqrt(rms);</div>
<div class="line">        <span class="keywordflow">if</span>(rms &lt;= errorBound)</div>
<div class="line">        {</div>
<div class="line">            count_time++;</div>
<div class="line">            <span class="keywordflow">if</span>(count_time &gt;= nb_steps_below_error)</div>
<div class="line">                <span class="keywordflow">break</span>;</div>
<div class="line">        }</div>
<div class="line">        <span class="keywordflow">else</span></div>
<div class="line">        {</div>
<div class="line">            count_time = 0;</div>
<div class="line">        }</div>
<div class="line"></div>
<div class="line">        printf(<span class="stringliteral">&quot;[%i] True state : %e %e %e %e %e %e, estimated : %e %e %e %e %e %e\n&quot;</span>, epoch,</div>
<div class="line">               xi-&gt;data[0],</div>
<div class="line">               xi-&gt;data[1],</div>
<div class="line">               xi-&gt;data[2],</div>
<div class="line">               xi-&gt;data[3],</div>
<div class="line">               xi-&gt;data[4],</div>
<div class="line">               xi-&gt;data[5],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[0],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[1],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[2],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[3],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[4],</div>
<div class="line">               s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[5]);</div>
<div class="line"></div>
<div class="line">        outfile &lt;&lt; epoch &lt;&lt; <span class="stringliteral">&quot; &quot;</span>;</div>
<div class="line">        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 6 ; ++i)</div>
<div class="line">            outfile &lt;&lt; xi-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 3 ; ++i)</div>
<div class="line">            outfile &lt;&lt; yi-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; 6 ; ++i)</div>
<div class="line">            outfile &lt;&lt; s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[i] &lt;&lt; <span class="stringliteral">&quot; &quot;</span> ;</div>
<div class="line">        outfile &lt;&lt; std::endl;</div>
<div class="line">        outfile_rms &lt;&lt; rms &lt;&lt; std::endl;</div>
<div class="line">    }</div>
<div class="line">    gettimeofday(&amp;after, NULL);</div>
<div class="line">    <span class="keywordtype">double</span> sbefore = before.tv_sec + before.tv_usec * 1E-6;</div>
<div class="line">    <span class="keywordtype">double</span> safter = after.tv_sec + after.tv_usec * 1E-6;</div>
<div class="line">    <span class="keywordtype">double</span> total = safter - sbefore;</div>
<div class="line"></div>
<div class="line">    outfile.close();</div>
<div class="line">    outfile_rms.close();</div>
<div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot; Run on &quot;</span> &lt;&lt; epoch &lt;&lt; <span class="stringliteral">&quot; epochs ;&quot;</span> &lt;&lt;  std::scientific &lt;&lt; total / double(epoch) &lt;&lt; <span class="stringliteral">&quot; s./step &quot;</span> &lt;&lt; std::endl;</div>
<div class="line">    printf(<span class="stringliteral">&quot;I found the following parameters : %e %e %e ; The true parameters being : %e %e %e \n&quot;</span>, s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[3], s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[4],s.<a class="code" href="structekf_1_1ekf__state.html#a13fc45f72e07e8396866ae3796095282">xk</a>-&gt;data[5],SIGMA, RHO, BETA);</div>
<div class="line"></div>
<div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot; Outputs are saved in example-009*.data &quot;</span> &lt;&lt; std::endl;</div>
<div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot; You can plot them using e.g. gnuplot : &quot;</span> &lt;&lt; std::endl;</div>
<div class="line">    <span class="comment">//std::cout &lt;&lt; &quot; gnuplot Data/plot-example-009.gplot ; gv Output/example-009-rms.ps ; gv Output/example-009-Lorentz.ps &quot; &lt;&lt; std::endl;</span></div>
<div class="line"></div>
<div class="line">    <span class="comment">/***********************************************/</span></div>
<div class="line">    <span class="comment">/****            Free the memory            ****/</span></div>
<div class="line">    <span class="comment">/***********************************************/</span></div>
<div class="line">    <a name="a14"></a><a class="code" href="namespaceekf.html#ad089f09f2d932343f311b16d414c1c6b">ekf_free</a>(p,s);</div>
<div class="line">}</div>
<div class="line"></div>
<div class="line"></div>
<div class="line"></div>
<div class="line"></div>
<div class="line"></div>
</div><!-- fragment --> </div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.6
</small></address>
</body>
</html>
